/*
 * MoBox2D - A MoSync SDK Port of Erin Catto's Box2D
 * 
 * JBox2D homepage: http://code.google.com/p/mobox2d/
 * Box2D homepage: http://www.box2d.org
 * MoSync homepage: http://mosync.com 
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

#ifndef B2_MATH_H
#define B2_MATH_H

#include <MoBox2D/Common/b2Settings.h>

#define M_PI           3.14159265358979323846  /* pi */
#define M_PI_2         1.57079632679489661923  /* pi/2 */
#define POLYNOM1(x, a)  ((a)[1]*(x)+(a)[0])
#define POLYNOM2(x, a)  (POLYNOM1((x),(a)+1)*(x)+(a)[0])
#define POLYNOM3(x, a)  (POLYNOM2((x),(a)+1)*(x)+(a)[0])
#define POLYNOM4(x, a)  (POLYNOM3((x),(a)+1)*(x)+(a)[0])
#define POLYNOM5(x, a)  (POLYNOM4((x),(a)+1)*(x)+(a)[0])
#define POLYNOM6(x, a)  (POLYNOM5((x),(a)+1)*(x)+(a)[0])
#define POLYNOM7(x, a)  (POLYNOM6((x),(a)+1)*(x)+(a)[0])
#define POLYNOM8(x, a)  (POLYNOM7((x),(a)+1)*(x)+(a)[0])
#define POLYNOM9(x, a)  (POLYNOM8((x),(a)+1)*(x)+(a)[0])
#define POLYNOM10(x, a) (POLYNOM9((x),(a)+1)*(x)+(a)[0])
#define POLYNOM11(x, a) (POLYNOM10((x),(a)+1)*(x)+(a)[0])
#define POLYNOM12(x, a) (POLYNOM11((x),(a)+1)*(x)+(a)[0])
#define POLYNOM13(x, a) (POLYNOM12((x),(a)+1)*(x)+(a)[0])

#define PIO4F   0.7853981633974483096F

/* These are for a 24-bit significand: */
static float DP1 = 0.78515625;
static float DP2 = 2.4187564849853515625e-4;
static float DP3 = 3.77489497744594108e-8;
static float lossth = 8192.;
static float T24M1 = 16777215.;
static float FOPI = 1.27323954473516;
static float sincof[] = {
	-1.9515295891E-4,
	 8.3321608736E-3,
	-1.6666654611E-1
};
static float coscof[] = {
	 2.443315711809948E-005,
	-1.388731625493765E-003,
	 4.166664568298827E-002
};


//#include <cmath>
//#include <cfloat>
//#include <cstddef>
//#include <limits>

/// This function is used to ensure that a floating point number is
/// not a NaN or infinity.
inline bool b2IsValid(float32 x)
{
	if (x != x)
	{
		// NaN.
		return false;
	}

	return true;
//	float32 infinity = std::numeric_limits<float32>::infinity();
//	return -infinity < x && x < infinity;
}

/// This is a approximate yet fast inverse square-root.
inline float32 b2InvSqrt(float32 x)
{
	union
	{
		float32 x;
		int32 i;
	} convert;

	convert.x = x;
	float32 xhalf = 0.5f * x;
	convert.i = 0x5f3759df - (convert.i >> 1);
	x = convert.x;
	x = x * (1.5f - xhalf * x * x);
	return x;
}

inline float frexpf(float x, int *i)
{
	int neg, j;

	j = 0;
	neg = 0;
	if (x<0)
	{
		x = -x;
		neg = 1;
	}
	if (x>=1.0)
	{
		while (x>=1.0)
		{
			j = j+1;
			x = x/2;
		}
	}
	else if (x < 0.5 && x != 0.0)
	{
		while(x<0.5)
		{
				j = j-1;
				x = 2*x;
		}
	}
	*i = j;
	if(neg)
	{
		x = -x;
	}
	return (x);
}

inline float ldexpf (float x, int d)
{
	for (; d > 0; d--)
	{
		x *= 2.0;
	}
	for (; d < 0; d++)
	{
		x *= 0.5;
	}
	return x;
}

inline float sqrtf(const float x)
{
    float f, y;
    int n;

    if (x==0.0)
    {
    	return x;
    }
    else if (x==1.0)
    {
    	return 1.0;
    }
    else if (x<0.0)
    {
//        errno=EDOM;
        return 0.0;
    }
    f=frexpf(x, &n);
    y=0.41731+0.59016*f; /*Educated guess*/
    /*For a 24 bit mantisa (float), two iterations are sufficient*/
    y+=f/y;
    y=ldexpf(y, -2) + f/y; /*Faster version of 0.25 * y + f/y*/

    if (n&1)
    {
        y*=0.7071067812;
        ++n;
    }
    return ldexpf(y, n/2);
}

inline double atan(double x)
{
	/*      Algorithm and coefficients from:
					"Software manual for the elementary functions"
					by W.J. Cody and W. Waite, Prentice-Hall, 1980
	*/
	double p[] = {
		-0.13688768894191926929e+2,
		-0.20505855195861651981e+2,
		-0.84946240351320683534e+1,
		-0.83758299368150059274e+0
	};
	double q[] = {
		 0.41066306682575781263e+2,
		 0.86157349597130242515e+2,
		 0.59578436142597344465e+2,
		 0.15024001160028576121e+2,
		 1.0
	};
	double a[] = {
		0.0,
		0.52359877559829887307710723554658381,  /* pi/6 */
		M_PI_2,
		1.04719755119659774615421446109316763   /* pi/3 */
	};
	int     neg = x < 0;
	int     n;
	double  g;
	if (x != x)
	{
//                errno = EDOM;
		return x;
	}
	if (neg)
	{
		x = -x;
	}
	if (x > 1.0)
	{
		x = 1.0/x;
		n = 2;
	}
	else
	{
		n = 0;
	}
	if (x > 0.26794919243112270647)
	{       /* 2-sqtr(3) */
		n = n + 1;
		x = (((0.73205080756887729353*x-0.5)-0.5)+x)/
			(1.73205080756887729353+x);
	}
	/* ??? avoid underflow ??? */
	g = x * x;
	x += x * g * POLYNOM3(g, p) / POLYNOM4(g, q);
	if (n > 1)
	{
		x = -x;
	}
	x += a[n];
	return neg ? -x : x;
}

inline float atan2f(const float x, const float y)
{
    double absx, absy, val;

    if (x == 0 && y == 0)
    {
 //           errno = EDOM;
		return 0;
    }
    absy = y < 0 ? -y : y;
    absx = x < 0 ? -x : x;
    if (absy - absx == absy)
    {
            /* x negligible compared to y */
		return y < 0 ? -M_PI_2 : M_PI_2;
    }
    if (absx - absy == absx)
    {
            /* y negligible compared to x */
		val = 0.0;
    }
    else
    {
    	val = atan(y/x);
    }
    if (x > 0)
    {
            /* first or fourth quadrant; already correct */
		return val;
    }
    if (y < 0)
    {
            /* third quadrant */
		return val - M_PI;
    }
    return val + M_PI;
}

inline float sinf( float xx )
{
	float *p;
	float x, y, z;
	register unsigned long j;
	register int sign;

	sign = 1;
	x = xx;
	if( xx < 0 )
	{
		sign = -1;
		x = -xx;
	}
	if( x > T24M1 )
	{
//        mtherr( "sinf", TLOSS );
		return(0.0);
	}
	j = (unsigned long)(FOPI * x); /* integer part of x/(PI/4) */
	y = j;
	/* map zeros to origin */
	if( j & 1 )
	{
		j += 1;
		y += 1.0;
	}
	j &= 7; /* octant modulo 360 degrees */
	/* reflect in x axis */
	if( j > 3)
	{
		sign = -sign;
		j -= 4;
	}

	if( x > lossth )
	{
//        mtherr( "sinf", PLOSS );
		x = x - y * PIO4F;
	}
	else
	{
		/* Extended precision modular arithmetic */
		x = ((x - y * DP1) - y * DP2) - y * DP3;
	}
	/*einits();*/
	z = x * x;
	if( (j==1) || (j==2) )
	{
		p = coscof;
		y = *p++;
		y = y * z + *p++;
		y = y * z + *p++;
		y *= z * z;
		y -= 0.5 * z;
		y += 1.0;
	}
	else
	{
		p = sincof;
		y = *p++;
		y = y * z + *p++;
		y = y * z + *p++;
		y *= z * x;
		y += x;
	}
	/*einitd();*/
	if(sign < 0)
	{
		y = -y;
	}
	return( y);
}

inline float cosf( float xx )
{
	float x, y, z;
	int j, sign;

	/* make argument positive */
	sign = 1;
	x = xx;
	if( x < 0 )
	{
		x = -x;
	}

	if( x > T24M1 )
	{
//        mtherr( "cosf", TLOSS );
		return(0.0);
	}

	j = (unsigned long)(FOPI * x); /* integer part of x/PIO4 */
	y = j;
	/* integer and fractional part modulo one octant */
	if( j & 1 )     /* map zeros to origin */
	{
		j += 1;
		y += 1.0;
	}
	j &= 7;
	if( j > 3)
	{
		j -=4;
		sign = -sign;
	}

	if( j > 1 )
	{
		sign = -sign;
	}

	if( x > lossth )
	{
//        mtherr( "cosf", PLOSS );
		x = x - y * PIO4F;
	}
	else
	{
		/* Extended precision modular arithmetic */
		x = ((x - y * DP1) - y * DP2) - y * DP3;
	}

	z = x * x;

	if( (j==1) || (j==2) )
	{
		y = (((-1.9515295891E-4 * z
			 + 8.3321608736E-3) * z
			 - 1.6666654611E-1) * z * x)
			 + x;
	}
	else
	{
		y = ((  2.443315711809948E-005 * z
		  - 1.388731625493765E-003) * z
		  + 4.166664568298827E-002) * z * z;
		y -= 0.5 * z;
		y += 1.0;
	}
	if(sign < 0)
	{
		y = -y;
	}
	return( y );
}

#define	b2Sqrt(x) 		sqrtf(x)
#define	b2Atan2(y, x)	atan2f(y, x)

inline float32 b2Abs(float32 a)
{
	return a > 0.0f ? a : -a;
}

/// A 2D column vector.
struct b2Vec2
{
	/// Default constructor does nothing (for performance).
	b2Vec2() {}

	/// Construct using coordinates.
	b2Vec2(float32 x, float32 y) : x(x), y(y) {}

	/// Set this vector to all zeros.
	void SetZero() { x = 0.0f; y = 0.0f; }

	/// Set this vector to some specified coordinates.
	void Set(float32 x_, float32 y_) { x = x_; y = y_; }

	/// Negate this vector.
	b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
	
	/// Read from and indexed element.
	float32 operator () (int32 i) const
	{
		return (&x)[i];
	}

	/// Write to an indexed element.
	float32& operator () (int32 i)
	{
		return (&x)[i];
	}

	/// Add a vector to this vector.
	void operator += (const b2Vec2& v)
	{
		x += v.x; y += v.y;
	}
	
	/// Subtract a vector from this vector.
	void operator -= (const b2Vec2& v)
	{
		x -= v.x; y -= v.y;
	}

	/// Multiply this vector by a scalar.
	void operator *= (float32 a)
	{
		x *= a; y *= a;
	}

	/// Get the length of this vector (the norm).
	float32 Length() const
	{
		return b2Sqrt(x * x + y * y);
	}

	/// Get the length squared. For performance, use this instead of
	/// b2Vec2::Length (if possible).
	float32 LengthSquared() const
	{
		return x * x + y * y;
	}

	/// Convert this vector into a unit vector. Returns the length.
	float32 Normalize()
	{
		float32 length = Length();
		if (length < b2_epsilon)
		{
			return 0.0f;
		}
		float32 invLength = 1.0f / length;
		x *= invLength;
		y *= invLength;

		return length;
	}

	/// Does this vector contain finite coordinates?
	bool IsValid() const
	{
		return b2IsValid(x) && b2IsValid(y);
	}

	float32 x, y;
};

/// A 2D column vector with 3 elements.
struct b2Vec3
{
	/// Default constructor does nothing (for performance).
	b2Vec3() {}

	/// Construct using coordinates.
	b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}

	/// Set this vector to all zeros.
	void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }

	/// Set this vector to some specified coordinates.
	void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }

	/// Negate this vector.
	b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }

	/// Add a vector to this vector.
	void operator += (const b2Vec3& v)
	{
		x += v.x; y += v.y; z += v.z;
	}

	/// Subtract a vector from this vector.
	void operator -= (const b2Vec3& v)
	{
		x -= v.x; y -= v.y; z -= v.z;
	}

	/// Multiply this vector by a scalar.
	void operator *= (float32 s)
	{
		x *= s; y *= s; z *= s;
	}

	float32 x, y, z;
};

/// A 2-by-2 matrix. Stored in column-major order.
struct b2Mat22
{
	/// The default constructor does nothing (for performance).
	b2Mat22() {}

	/// Construct this matrix using columns.
	b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
	{
		col1 = c1;
		col2 = c2;
	}

	/// Construct this matrix using scalars.
	b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
	{
		col1.x = a11; col1.y = a21;
		col2.x = a12; col2.y = a22;
	}

	/// Construct this matrix using an angle. This matrix becomes
	/// an orthonormal rotation matrix.
	explicit b2Mat22(float32 angle)
	{
		// TODO_ERIN compute sin+cos together.
		float32 c = cosf(angle), s = sinf(angle);
		col1.x = c; col2.x = -s;
		col1.y = s; col2.y = c;
	}

	/// Initialize this matrix using columns.
	void Set(const b2Vec2& c1, const b2Vec2& c2)
	{
		col1 = c1;
		col2 = c2;
	}

	/// Initialize this matrix using an angle. This matrix becomes
	/// an orthonormal rotation matrix.
	void Set(float32 angle)
	{
		float32 c = cosf(angle), s = sinf(angle);
		col1.x = c; col2.x = -s;
		col1.y = s; col2.y = c;
	}

	/// Set this to the identity matrix.
	void SetIdentity()
	{
		col1.x = 1.0f; col2.x = 0.0f;
		col1.y = 0.0f; col2.y = 1.0f;
	}

	/// Set this matrix to all zeros.
	void SetZero()
	{
		col1.x = 0.0f; col2.x = 0.0f;
		col1.y = 0.0f; col2.y = 0.0f;
	}

	/// Extract the angle from this matrix (assumed to be
	/// a rotation matrix).
	float32 GetAngle() const
	{
		return b2Atan2(col1.y, col1.x);
	}

	b2Mat22 GetInverse() const
	{
		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
		b2Mat22 B;
		float32 det = a * d - b * c;
		if (det != 0.0f)
		{
			det = 1.0f / det;
		}
		B.col1.x =  det * d;	B.col2.x = -det * b;
		B.col1.y = -det * c;	B.col2.y =  det * a;
		return B;
	}

	/// Solve A * x = b, where b is a column vector. This is more efficient
	/// than computing the inverse in one-shot cases.
	b2Vec2 Solve(const b2Vec2& b) const
	{
		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
		float32 det = a11 * a22 - a12 * a21;
		if (det != 0.0f)
		{
			det = 1.0f / det;
		}
		b2Vec2 x;
		x.x = det * (a22 * b.x - a12 * b.y);
		x.y = det * (a11 * b.y - a21 * b.x);
		return x;
	}

	b2Vec2 col1, col2;
};

/// A 3-by-3 matrix. Stored in column-major order.
struct b2Mat33
{
	/// The default constructor does nothing (for performance).
	b2Mat33() {}

	/// Construct this matrix using columns.
	b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
	{
		col1 = c1;
		col2 = c2;
		col3 = c3;
	}

	/// Set this matrix to all zeros.
	void SetZero()
	{
		col1.SetZero();
		col2.SetZero();
		col3.SetZero();
	}

	/// Solve A * x = b, where b is a column vector. This is more efficient
	/// than computing the inverse in one-shot cases.
	b2Vec3 Solve33(const b2Vec3& b) const;

	/// Solve A * x = b, where b is a column vector. This is more efficient
	/// than computing the inverse in one-shot cases. Solve only the upper
	/// 2-by-2 matrix equation.
	b2Vec2 Solve22(const b2Vec2& b) const;

	b2Vec3 col1, col2, col3;
};

/// A transform contains translation and rotation. It is used to represent
/// the position and orientation of rigid frames.
struct b2Transform
{
	/// The default constructor does nothing (for performance).
	b2Transform() {}

	/// Initialize using a position vector and a rotation matrix.
	b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}

	/// Set this to the identity transform.
	void SetIdentity()
	{
		position.SetZero();
		R.SetIdentity();
	}

	/// Set this based on the position and angle.
	void Set(const b2Vec2& p, float32 angle)
	{
		position = p;
		R.Set(angle);
	}

	/// Calculate the angle that the rotation matrix represents.
	float32 GetAngle() const
	{
		return b2Atan2(R.col1.y, R.col1.x);
	}

	b2Vec2 position;
	b2Mat22 R;
};

/// This describes the motion of a body/shape for TOI computation.
/// Shapes are defined with respect to the body origin, which may
/// no coincide with the center of mass. However, to support dynamics
/// we must interpolate the center of mass position.
struct b2Sweep
{
	/// Get the interpolated transform at a specific time.
	/// @param alpha is a factor in [0,1], where 0 indicates t0.
	void GetTransform(b2Transform* xf, float32 alpha) const;

	/// Advance the sweep forward, yielding a new initial state.
	/// @param t the new initial time.
	void Advance(float32 t);

	/// Normalize the angles.
	void Normalize();

	b2Vec2 localCenter;	///< local center of mass position
	b2Vec2 c0, c;		///< center world positions
	float32 a0, a;		///< world angles
};


extern const b2Vec2 b2Vec2_zero;
extern const b2Mat22 b2Mat22_identity;
extern const b2Transform b2Transform_identity;

/// Perform the dot product on two vectors.
inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
{
	return a.x * b.x + a.y * b.y;
}

/// Perform the cross product on two vectors. In 2D this produces a scalar.
inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
{
	return a.x * b.y - a.y * b.x;
}

/// Perform the cross product on a vector and a scalar. In 2D this produces
/// a vector.
inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
{
	return b2Vec2(s * a.y, -s * a.x);
}

/// Perform the cross product on a scalar and a vector. In 2D this produces
/// a vector.
inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
{
	return b2Vec2(-s * a.y, s * a.x);
}

/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
{
	return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
}

/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another (inverse transform).
inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
{
	return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
}

/// Add two vectors component-wise.
inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(a.x + b.x, a.y + b.y);
}

/// Subtract two vectors component-wise.
inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(a.x - b.x, a.y - b.y);
}

inline b2Vec2 operator * (float32 s, const b2Vec2& a)
{
	return b2Vec2(s * a.x, s * a.y);
}

inline bool operator == (const b2Vec2& a, const b2Vec2& b)
{
	return a.x == b.x && a.y == b.y;
}

inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
{
	b2Vec2 c = a - b;
	return c.Length();
}

inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
{
	b2Vec2 c = a - b;
	return b2Dot(c, c);
}

inline b2Vec3 operator * (float32 s, const b2Vec3& a)
{
	return b2Vec3(s * a.x, s * a.y, s * a.z);
}

/// Add two vectors component-wise.
inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}

/// Subtract two vectors component-wise.
inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}

/// Perform the dot product on two vectors.
inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
{
	return a.x * b.x + a.y * b.y + a.z * b.z;
}

/// Perform the cross product on two vectors.
inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}

inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
{
	return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
}

// A * B
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
{
	return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
}

// A^T * B
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
{
	b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
	b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
	return b2Mat22(c1, c2);
}

/// Multiply a matrix times a vector.
inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
{
	return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
}

inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
{
	float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
	float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;

	return b2Vec2(x, y);
}

inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
{
	return b2MulT(T.R, v - T.position);
}

inline b2Vec2 b2Abs(const b2Vec2& a)
{
	return b2Vec2(b2Abs(a.x), b2Abs(a.y));
}

inline b2Mat22 b2Abs(const b2Mat22& A)
{
	return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
}

template <typename T>
inline T b2Min(T a, T b)
{
	return a < b ? a : b;
}

inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
}

template <typename T>
inline T b2Max(T a, T b)
{
	return a > b ? a : b;
}

inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
}

template <typename T>
inline T b2Clamp(T a, T low, T high)
{
	return b2Max(low, b2Min(a, high));
}

inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
{
	return b2Max(low, b2Min(a, high));
}

template<typename T> inline void b2Swap(T& a, T& b)
{
	T tmp = a;
	a = b;
	b = tmp;
}

/// "Next Largest Power of 2
/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
/// largest power of 2. For a 32-bit value:"
inline uint32 b2NextPowerOfTwo(uint32 x)
{
	x |= (x >> 1);
	x |= (x >> 2);
	x |= (x >> 4);
	x |= (x >> 8);
	x |= (x >> 16);
	return x + 1;
}

inline bool b2IsPowerOfTwo(uint32 x)
{
	bool result = x > 0 && (x & (x - 1)) == 0;
	return result;
}

inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
{
	xf->position = (1.0f - alpha) * c0 + alpha * c;
	float32 angle = (1.0f - alpha) * a0 + alpha * a;
	xf->R.Set(angle);

	// Shift to origin
	xf->position -= b2Mul(xf->R, localCenter);
}

inline void b2Sweep::Advance(float32 t)
{
	c0 = (1.0f - t) * c0 + t * c;
	a0 = (1.0f - t) * a0 + t * a;
}

inline float modff(float x, float * y)
{
    *y=(long)x;
    return (x-*y);
}

inline float floorf(float x)
{
	float f, y;

	if (x > -1.0 && x < 1.0)
	{
		return (x >= 0 ? 0 : -1.0);
	}

	y = modff (x, &f);

	if (y == 0.0)
	{
		return (x);
	}

	return (x >= 0 ? f : f - 1.0);
}

/// Normalize an angle in radians to be between -pi and pi
inline void b2Sweep::Normalize()
{
	float32 twoPi = 2.0f * b2_pi;
	float32 d =  twoPi * floorf(a0 / twoPi);
	a0 -= d;
	a -= d;
}

#endif
